﻿#pragma once

#include <learn/alg/alg_equation.h>
#include <learn/geom/geom_bezier_curve.h>
#include <learn/geom/geom_bezier_surface.h>
#include <learn/geom/geom_bspline_curve.h>
#include <learn/geom/geom_bspline_surface.h>
#include <learn/math/matrix.h>

namespace xi
{

namespace alg
{

using namespace xi::math;
using namespace xi::geom;

template <std::size_t N> struct Alg_Projection
{
    using Vec = VecT<N, double>;

    /**
     * @brief 最近点投影算法
     *
     * @param[in] curve
     * @param[in] P
     * @param[in] t
     * @param[in] tol
     * @return double
     */
    static double nearest(std::shared_ptr<Geom_Curve<N>> curve, const Vec &P, double t, double tol)
    {
        // 对每个有效节点区间等分寻找最近点
        int times = 0;
        double u0 = curve->first_param();
        double u1 = curve->last_param();

        // 最近点投影不稳定，因此需要细分足够多次
        double near = P.distance(curve->value(t));
        int N = (u1 - u0) * 10;
        for (int i = 0; i < N; i++)
        {
            double u = u0 + (u1 - u0) / (N - 1) * i;
            auto Q = curve->value(u);
            double d = P.distance(Q);
            if (d < near)
            {
                near = d;
                t = u;
            }
        }

        // 应用牛顿迭代法
        double dp = tol * 10;
        while (near > tol && dp > tol && times++ < Numeric::iter)
        {
            auto [Q, V1, V2] = curve->d2(t);
            double u = t - dot(V1, Q - P) / (dot(V2, Q - P) + V1.square_modulus());

            u = std::max(u, u0 + std::numeric_limits<double>::epsilon());
            u = std::min(u, u1 - std::numeric_limits<double>::epsilon());

            near = P.distance(curve->value(u));
            dp = V1.modulus() * std::abs(u - t);
            t = u;
        }
        return t;
    }

    /**
     * @brief 平行投影算法
     *
     * @param[in] curve
     * @param[in] P
     * @param[in] dir
     * @param[in] t
     * @param[in] tol
     * @return double
     */
    static double parallel(std::shared_ptr<Geom_Curve<N>> curve, const Vec &P, Vec dir, double t, double tol)
    {
        int times = 0;
        double u0 = curve->first_param();
        double u1 = curve->last_param();

        double angle = cross((P - curve->value(t)).normalize(), dir.normalize()).modulus();
        int N = (u1 - u0) * 10;
        for (int i = 0; i < N; i++)
        {
            double u = u0 + (u1 - u0) / (N - 1) * i;
            double a = cross((P - curve->value(u)).normalize(), dir).modulus();
            if (a < angle)
            {
                angle = a;
                t = u;
            }
        }

        // 应用牛顿迭代法
        double dp = tol * 10;
        while (angle > tol && dp > tol && times++ < Numeric::iter)
        {
            auto [Q, V1] = curve->d1(t);
            auto D = cross(Q - P, dir);
            double u = t - D.square_modulus() / dot(cross(V1 - P, dir * 2), D);

            u = std::max(u, u0 + std::numeric_limits<double>::epsilon());
            u = std::min(u, u1 - std::numeric_limits<double>::epsilon());

            angle = cross((P - curve->value(u)).normalize(), dir.normalize()).modulus();
            dp = V1.modulus() * std::abs(u - t);
            t = u;
        }
        return t;
    }

    /**
     * @brief 切线投影算法
     *
     * @param[in] curve
     * @param[in] P
     * @param[in] t
     * @param[in] tol
     * @return double
     */
    static double orthogonal1(std::shared_ptr<Geom_Curve<N>> curve, const Vec &P, double t, double tol)
    {
        // 使用切线进行投影
        int times = 0;
        double u0 = curve->first_param();
        double u1 = curve->last_param();

        double angle = tol * 10;
        while (angle > tol && times++ < Numeric::iter)
        {
            auto [C0, V1] = curve->d1(t);

            // 距离足够小直接退出
            if (P.distance(C0) < tol)
                break;

            // 计算 P 到切线上的投影点 Q，得到增量一阶方法
            auto T = V1.normalized();
            angle = std::abs(dot(T, (P - C0).normalize()));
            if (angle < tol)
                break;

            auto Q = C0 + T * dot(P - C0, T);
            double dt = dot(V1, Q - C0) / V1.square_modulus();

            t = std::max(std::min(t + dt, u1 - std::numeric_limits<double>::epsilon()),
                         u0 + std::numeric_limits<double>::epsilon());
        }
        return t;
    }

    /**
     * @brief 曲率圆投影算法
     *
     * @param[in] curve
     * @param[in] P
     * @param[in] t
     * @param[in] tol
     * @return double
     */
    static double orthogonal2(std::shared_ptr<Geom_Curve<N>> curve, const Vec &P, double t, double tol)
    {
        // 使用曲率圆进行投影
        int times = 0;
        double u0 = curve->first_param();
        double u1 = curve->last_param();

        double angle = tol * 10;
        while (angle > tol && times++ < Numeric::iter)
        {
            auto [C0, V1, V2] = curve->d2(t);

            // 距离足够小直接退出
            if (P.distance(C0) < tol)
                break;

            // Frenet 标架
            auto T = V1.normalized();
            angle = std::abs(dot(T, (P - C0).normalize()));
            if (angle < tol)
                break;

            auto B = cross(V1, V2).normalize();
            auto N = cross(B, T);

            // 考虑曲率为 0 的情况
            auto vm3 = std::pow(V1.modulus(), 3);
            double kappa = cross(V1, V2).modulus() / vm3;
            if (kappa < std::numeric_limits<double>::epsilon())
                return nearest(curve, P, t, tol);

            // 计算 P 到曲率圆上的投影点 Q，得到增量二阶方法
            auto O = C0 + N / kappa;
            auto L = cross(cross(B, P - O).normalize(), B);
            auto Q = O + L / kappa;

            // 计算增量
            double dt = std::sqrt(2 * cross(V1, Q - C0).modulus() / kappa / vm3);
            dt = std::abs(dt) * sign(dot(V1, Q - C0));

            t = std::max(std::min(t + dt, u1 - std::numeric_limits<double>::epsilon()),
                         u0 + std::numeric_limits<double>::epsilon());
        }
        return t;
    }

    /**
     * @brief 最近点投影算法
     *
     * @param[in] surface
     * @param[in] P
     * @param[in] u
     * @param[in] v
     * @param[in] tol
     * @return Vec2
     */
    static Vec2 nearest(std::shared_ptr<Geom_Surface<N>> surface, const Vec &P, double u, double v, double tol)
    {
        // 应用牛顿迭代法
        int times = 0;
        auto [U1, U2, V1, V2] = surface->bounds();

        double dp = tol * 10;
        double near = P.distance(surface->value(u, v));
        while (near > tol && dp > tol && times++ < Numeric::iter)
        {
            auto [Q, DU, DV, DUU, DVV, DUV] = surface->d2(u, v);

            auto dS = Q - P;
            double lSu = DU.modulus();
            double lSv = DV.modulus();

            double J[4];
            J[0] = std::pow(lSu, 2) + dot(DUU, dS);
            J[1] = J[2] = dot(DU, DV) + dot(DUV, dS);
            J[3] = std::pow(lSv, 2) + dot(DVV, dS);

            double kappa[2];
            kappa[0] = -dot(DU, dS);
            kappa[1] = -dot(DV, dS);

            solve(J, kappa);
            u += kappa[0];
            v += kappa[1];

            // 防止越界
            u = std::max(std::min(u, U2), U1);
            v = std::max(std::min(v, V2), V1);

            near = P.distance(surface->value(u, v));
            dp = std::abs(lSu * kappa[0] + lSv * kappa[1]);
        }

        return Vec2{u, v};
    }

    /**
     * @brief 平行投影算法
     *
     * @param[in] surface
     * @param[in] P
     * @param[in] dir
     * @param[in] u
     * @param[in] v
     * @param[in] tol
     * @return Vec2
     */
    static Vec2 parallel(std::shared_ptr<Geom_Surface<N>> surface, const Vec &P, Vec dir, double u, double v,
                         double tol)
    {
        // 切平面投影
        int times = 0;
        double angle = tol * 10;
        auto [U1, U2, V1, V2] = surface->bounds();

        while (angle > tol && times++ < Numeric::iter)
        {
            // 计算各阶导向量
            auto [Q, S10, S01] = surface->d1(u, v);
            auto N = cross(S10, S01).normalize();

            // 投影方向，如果距离足够小就退出
            auto dP = P - Q;
            angle = cross(dir, dP.normalized()).modulus();
            if (dP.modulus() < tol || angle < tol)
                break;

            // 计算 P 沿着 dir 投影到切平面上的点
            auto C1 = P - dir * std::abs(dot(dP, N)) / std::abs(dot(dir, N)) * sign(dot(dir, dP)) - Q;

            double I[4];
            I[0] = dot(S10, S10);
            I[1] = I[2] = dot(S10, S01);
            I[3] = dot(S01, S01);

            double b[2];
            b[0] = dot(C1, S10);
            b[1] = dot(C1, S01);

            solve(I, b);
            u += b[0];
            v += b[1];

            // 防止越界
            u = std::max(std::min(u, U2), U1);
            v = std::max(std::min(v, V2), V1);
        }

        return Vec2{u, v};
    }

    /**
     * @brief 切平面投影算法
     *
     * @param[in] surface
     * @param[in] P
     * @param[in] u
     * @param[in] v
     * @param[in] tol
     * @return Vec2
     */
    static Vec2 orthogonal1(std::shared_ptr<Geom_Surface<N>> surface, const Vec &P, double u, double v, double tol)
    {
        // 切平面投影
        int times = 0;
        double angle = tol * 10;
        auto [U1, U2, V1, V2] = surface->bounds();

        while (angle > tol && times++ < Numeric::iter)
        {
            // 计算各阶导向量
            auto [Q, S10, S01] = surface->d1(u, v);
            auto N = cross(S10, S01).normalize();

            // 投影方向，如果距离足够小就退出
            auto dP = P - Q;
            if (dP.modulus() < tol)
                break;

            angle = cross(N, dP.normalized()).modulus();
            if (angle < tol)
                break;

            // 计算投影向量
            auto C1 = dP - N * dot(dP, N);

            double I[4];
            I[0] = dot(S10, S10);
            I[1] = I[2] = dot(S10, S01);
            I[3] = dot(S01, S01);

            double b[2];
            b[0] = dot(C1, S10);
            b[1] = dot(C1, S01);

            solve(I, b);
            u += b[0];
            v += b[1];

            // 防止越界
            u = std::max(std::min(u, U2), U1);
            v = std::max(std::min(v, V2), V1);
        }

        return Vec2{u, v};
    }

    /**
     * @brief 曲率投影算法
     *
     * @param[in] surface
     * @param[in] P
     * @param[in] u
     * @param[in] v
     * @param[in] tol
     * @return Vec2
     */
    static Vec2 orthogonal2(std::shared_ptr<Geom_Surface<N>> surface, const Vec &P, double u, double v, double tol)
    {
        // 曲率投影
        int times = 0;
        double angle = tol * 10;

        auto [U1, U2, V1, V2] = surface->bounds();

        while (angle > tol && times++ < Numeric::iter)
        {
            // 计算各阶导向量
            auto [Q, S10, S01, S20, S02, S11] = surface->d2(u, v);

            // 获得 I,II 基本形式的矩阵
            auto N = cross(S10, S01).normalize();

            // 投影方向，如果距离足够小就退出
            auto dP = P - Q;
            if (dP.modulus() < tol)
                break;

            angle = cross(N, dP.normalized()).modulus();
            if (angle < tol)
                break;

            double I[4];
            I[0] = dot(S10, S10);
            I[1] = I[2] = dot(S10, S01);
            I[3] = dot(S01, S01);

            double II[4];
            II[0] = dot(N, S20);
            II[1] = II[2] = dot(N, S11);
            II[3] = dot(N, S02);

            // 沿着 Su Sv 方向进行分解（利用线性方程组）
            double lambda[2];
            lambda[0] = dot(dP, S10);
            lambda[1] = dot(dP, S01);

            // Cholesky 分解进行求解
            solve(I, lambda);

            // 计算法曲率
            double h = 0;
            for (int i = 0; i < 2; i++)
                for (int j = 0; j < 2; j++)
                    h += II[i * 2 + j] * lambda[i] * lambda[j];

            double g = 0;
            for (int i = 0; i < 2; i++)
                for (int j = 0; j < 2; j++)
                    g += I[i * 2 + j] * lambda[i] * lambda[j];

            // 法曲率，法曲率圆心，投影点 Q
            double kappa = h / g;

            // 如果遇到曲率为零，就使用最近点投影（稳定性不太好）
            if (std::abs(kappa) < std::numeric_limits<double>::epsilon())
                return nearest(surface, P, u, v, tol);

            double r = std::abs(1 / kappa);
            auto O = Q + N / kappa;
            auto Q1 = O + (P - O).normalize() * r;

            // 计算增量
            auto C1 = S10 * lambda[0] + S01 * lambda[1];
            double dt = std::sqrt(2 * cross(Q1 - Q, C1).modulus() * r / std::pow(C1.modulus(), 3));
            dt = std::abs(dt) * sign(dot(C1, Q1 - Q));

            u += dt * lambda[0];
            v += dt * lambda[1];

            u = std::max(std::min(u, U2), U1);
            v = std::max(std::min(v, V2), V1);
        }

        return Vec2{u, v};
    }

    /**
     * @brief 圆环投影算法
     *
     * @param[in] surface
     * @param[in] P
     * @param[in] u
     * @param[in] v
     * @param[in] tol
     * @return Vec2
     */
    static Vec2 torus(std::shared_ptr<Geom_Surface<N>> surface, const Vec &P, double u, double v, double tol)
    {
        // 曲率投影
        int times = 0;
        double angle = tol * 10;
        auto [U1, U2, V1, V2] = surface->bounds();

        while (angle > tol && times++ < Numeric::iter)
        {
            // 计算各阶导向量
            auto [Q, S10, S01, S20, S02, S11] = surface->d2(u, v);

            // 距离足够小就退出
            if (P.distance(Q) < tol)
                break;

            // 获得 I,II 基本形式的矩阵
            auto N = cross(S10, S01).normalize();
            MatrixXd I(2, 2);
            MatrixXd II(2, 2);

            I(0, 0) = dot(S10, S10);
            I(0, 1) = I(1, 0) = dot(S10, S01);
            I(1, 1) = dot(S01, S01);

            II(0, 0) = dot(N, S20);
            II(0, 1) = II(1, 0) = dot(N, S11);
            II(1, 1) = dot(N, S02);

            // 算出主曲率和主方向
            double detI = I(0, 0) * I(1, 1) - I(0, 1) * I(1, 0);
            double detII = II(0, 0) * II(1, 1) - II(0, 1) * II(1, 0);

            double H = (II(0, 0) * I(1, 1) - 2 * II(1, 0) * I(1, 0) + II(1, 1) * I(0, 0)) / detI / 2;
            double K = detII / detI;

            // 如果当前位置是脐点，就转而使用法曲率圆投影
            double Delta = H * H - K;
            if (Delta < 0)
                return orthogonal2(surface, P, u, v, tol);

            // 主曲率
            double k1 = H + std::sqrt(Delta);
            double k2 = H - std::sqrt(Delta);

            // 防止环面半径过大
            if (std::min(std::abs(k1), std::abs(k2)) <= std::numeric_limits<double>::epsilon())
                return orthogonal2(surface, P, u, v, tol);

            // 主方向
            Vec e1, e2;
            {
                auto div1 = II(0, 0) - k1 * I(0, 0);
                auto div2 = II(1, 0) - k1 * I(1, 0);
                if (std::abs(div1) > std::numeric_limits<double>::epsilon())
                    e1 = S10 * div2 / div1 - S01;
                else if (std::abs(div2) > std::numeric_limits<double>::epsilon())
                    e1 = -S10 + S01 * div1 / div2;
                else
                    return orthogonal2(surface, P, u, v, tol);
            }

            {
                auto div1 = II(0, 0) - k2 * I(0, 0);
                auto div2 = II(1, 0) - k2 * I(1, 0);
                if (std::abs(div1) > std::numeric_limits<double>::epsilon())
                    e2 = S10 * div2 / div1 - S01;
                else if (std::abs(div2) > std::numeric_limits<double>::epsilon())
                    e2 = -S10 + S01 * div1 / div2;
                else
                    return orthogonal2(surface, P, u, v, tol);
            }

            e1.normalize();
            e2.normalize();

            // 确保 |k1| <= |k2|
            if (std::abs(k1) > std::abs(k2))
            {
                std::swap(k1, k2);
                std::swap(e1, e2);
            }

            // 确保 k1 <= 0
            if (k1 > 0)
            {
                N = -N;
                k1 = -k1;
                k2 = -k2;
            }

            // 建立环面，得到投影点 Pr
            double R = -1 / k1 + 1 / k2;
            double r = std::abs(1 / k2);
            auto O = Q + N / k1;
            auto OP = P - O;
            auto OB = (OP - e2 * dot(OP, e2)).normalize() * R;
            auto Pr = O + OB + (OP - OB).normalize() * r;

            // 计算初始增量
            auto QPr = Pr - Q;

            // 投影距离足够小
            if (QPr.modulus() < tol)
                break;

            // 夹角足够小
            angle = cross(N, QPr.normalized()).modulus();
            if (angle < tol)
                break;

            VectorXd duv(2);
            duv[0] = dot(QPr, S10);
            duv[1] = dot(QPr, S01);

            solve(I.data(), duv.data());

            // 使用牛顿迭代法计算根
            auto f = [&](VectorXd UV) -> VectorXd {
                auto E = S10 * UV[0] + S01 * UV[1] +
                         (S20 * UV[0] * UV[0] + S11 * UV[0] * UV[1] * 2 + S02 * UV[1] * UV[1]) / 2 - QPr;
                return {E.x(), E.y(), E.z()};
            };

            // 获得增量
            duv = Alg_RootOfEquation::solve(f, duv);
            u += duv[0];
            v += duv[1];

            // 防止越界
            u = std::max(std::min(u, U2), U1);
            v = std::max(std::min(v, V2), V1);
        }

        return Vec2{u, v};
    }
};

} // namespace alg

} // namespace xi
